﻿Imports System.Math

'以弦长为参数的三次自然样条的好算法！

Public Class interpolate

    'u1,v1好像是插值后的结果
    Private u1(-1) As Double, v1(-1) As Double, num As Integer


    '插值函数
    Private Function insert(ByVal pt() As Point3D) As Point3D()

        Dim count As Integer
        count = pt.Length - 1


        Dim t(count) As Double
        Try
            For i As Integer = 1 To count

                t(i) = tosqrt(pt(i).x - pt(i - 1).x, pt(i).y - pt(i - 1).y)

            Next



            '抛物条件
            Dim u(2) As Double
            Dim tx1 As Double, ty1 As Double, tx2 As Double, ty2 As Double

            '防止分母出现0的情况，则返回原来的点，不进行插值

            u(0) = (pt(1).x - pt(0).x) / t(1) : u(1) = (pt(2).x - pt(1).x) / t(2)
            u(2) = (u(1) - u(0)) / (t(2) + t(1))
            tx1 = u(0) - u(2) * t(1)
            u(0) = (pt(1).y - pt(0).y) / t(1) : u(1) = (pt(2).y - pt(1).y) / t(2)
            u(2) = (u(1) - u(0)) / (t(2) + t(1))
            ty1 = u(0) - u(2) * t(1)
            u(0) = (pt(count).x - pt(count - 1).x) / t(count) : u(1) = (pt(count - 1).x - pt(count - 2).x) / t(count - 1)
            u(2) = (u(0) - u(1)) / (t(count) + t(count - 1))
            tx2 = u(0) + u(2) * t(count)
            u(0) = (pt(count).y - pt(count - 1).y) / t(count) : u(1) = (pt(count - 1).y - pt(count - 2).y) / t(count - 1)
            u(2) = (u(0) - u(1)) / (t(count) + t(count - 1))
            ty2 = u(0) + u(2) * t(count)


            '求方程组系数和常数
            Dim a(count) As Double, b(count) As Double, c(count) As Double
            Dim dx(count) As Double, dy(count) As Double

            For i As Integer = 1 To count - 1

                a(i) = 2 * (t(i) + t(i + 1)) : b(i) = t(i + 1) : c(i) = t(i)
                dx(i) = 3 * (t(i) * (pt(i + 1).x - pt(i).x) / t(i + 1) + t(i + 1) * (pt(i).x - pt(i - 1).x) / t(i))
                dy(i) = 3 * (t(i) * (pt(i + 1).y - pt(i).y) / t(i + 1) + t(i + 1) * (pt(i).y - pt(i - 1).y) / t(i))

            Next




            '采用追赶法解方程组
            Dim qx(count) As Double, qy(count) As Double

            If a(0) = 0 Then a(0) = 1.0E-20
            c(0) = c(0) / a(0)
            For i As Integer = 1 To count - 1
                a(i) = a(i) - b(i) * c(i - 1) : c(i) = c(i) / a(i)
            Next i
            a(count) = a(count) - b(count) * c(count - 2)
            qx(0) = dx(0) / a(0) : qy(0) = dy(0) / a(0)
            For i As Integer = 1 To count
                If a(i) = 0 Then a(i) = 1.0E-20
                qx(i) = (dx(i) - b(i) * qx(i - 1)) / a(i)
                qy(i) = (dy(i) - b(i) * qy(i - 1)) / a(i)
            Next i

            Dim px(count) As Double, py(count) As Double
            px(count) = qx(count) : py(count) = qy(count)

            For i As Integer = count - 1 To 0 Step -1
                px(i) = qx(i) - c(i) * px(i + 1)
                py(i) = qy(i) - c(i) * py(i + 1)
            Next i



            '计算曲线上点的坐标
            Dim minx As Double, miny As Double, maxx As Double, maxy As Double

            minx = pt(0).x : maxx = pt(0).x
            miny = pt(0).y : maxy = pt(0).y
            For i As Integer = 1 To pt.Length - 1
                If pt(i).x < minx Then minx = pt(i).x
                If pt(i).x > maxx Then maxx = pt(i).x
                If pt(i).y > miny Then miny = pt(i).y
                If pt(i).y < maxy Then maxy = pt(i).y
            Next


            'Edge(pt, minx, miny, maxx, maxy)

            Dim bx3 As Double, bx4 As Double, by3 As Double, by4 As Double, tt As Double
            Dim cx As Double, cy As Double

            For i As Integer = 0 To count - 1
                bx3 = (3 * (pt(i + 1).x - pt(i).x) / t(i + 1) - 2 * px(i) - px(i + 1)) / t(i + 1)
                bx4 = ((2 * (pt(i).x - pt(i + 1).x) / t(i + 1) + px(i) + px(i + 1)) / t(i + 1)) / t(i + 1)
                by3 = (3 * (pt(i + 1).y - pt(i).y) / t(i + 1) - 2 * py(i) - py(i + 1)) / t(i + 1)
                by4 = ((2 * (pt(i).y - pt(i + 1).y) / t(i + 1) + py(i) + py(i + 1)) / t(i + 1)) / t(i + 1)
                tt = 0
                While (tt <= t(i + 1))
                    cx = pt(i).x + (px(i) + (bx3 + bx4 * tt) * tt) * tt
                    cy = pt(i).y + (py(i) + (by3 + by4 * tt) * tt) * tt

                    ReDim Preserve u1(u1.Length)
                    ReDim Preserve v1(v1.Length)

                    u1(num) = cx : v1(num) = cy : num = num + 1 : tt = tt + (maxx - minx) / ((count + 1) * 1.5)
                End While
                ReDim Preserve u1(u1.Length)
                ReDim Preserve v1(v1.Length)
                u1(num) = pt(i + 1).x : v1(num) = pt(i + 1).y : num = num + 1
            Next i

            'Edge1(u1, v1, minx, maxy, maxx, miny)

            'Dim eWidth As Double, eHight As Double
            'eWidth = maxx - minx
            'eHight = maxy - miny

            Dim ResultPt(u1.Length - 1) As Point3D
            For i As Integer = 0 To u1.Length - 1
                ResultPt(i) = New Point3D
                ResultPt(i).x = u1(i)
                ResultPt(i).y = v1(i)
            Next

            Return ResultPt
        Catch ex As Exception
            Return pt
            Exit Function
        End Try

    End Function



    Private Function tosqrt(ByVal x As Double, ByVal y As Double) As Double    '求平方根

        Return Sqrt(x ^ 2 + y ^ 2)

    End Function


    Private Sub Edge(ByVal pt() As Point3D, ByVal minx As Double, ByVal miny As Double, ByVal maxx As Double, ByVal maxy As Double)
        ' 点位置

        minx = pt(0).X : maxx = pt(0).X
        miny = pt(0).Y : maxy = pt(0).Y
        For i As Integer = 1 To pt.Length - 1
            If pt(i).X < minx Then minx = pt(i).X
            If pt(i).X > maxx Then maxx = pt(i).X
            If pt(i).Y > miny Then miny = pt(i).Y
            If pt(i).Y < maxy Then maxy = pt(i).Y
        Next
    End Sub

    Private Sub Edge1(ByVal x() As Double, ByVal y() As Double, ByVal eLeft As Double, ByVal eTop As Double, ByVal eRight As Double, ByVal eButtom As Double)
        eLeft = x(0) : eRight = x(0)
        eTop = y(0) : eRight = y(0)
        For i As Integer = 1 To UBound(x)
            If x(i) < eLeft Then eLeft = x(i)
            If x(i) > eRight Then eRight = x(i)
            If y(i) > eTop Then eTop = y(i)
            If y(i) < eButtom Then eButtom = y(i)
        Next
    End Sub


    ''' <summary>
    ''' 曲线插值
    ''' </summary>
    ''' <param name="pt">要插值的点</param>
    ''' <param name="IsBlock">true表示要封闭曲线，false表示要不封闭的曲线</param>
    ''' <returns></returns>
    ''' <remarks></remarks>
    Public Function BlockSmooth(ByVal pt() As Point3D, ByVal IsBlock As Boolean) As Point3D()
        '为了使曲线润滑，对pt进行两次（其实多次更好）插值，

        '首先判断要插值的点是不是大于2个，如果小于3个则直接返回，不进行插值处理
        If pt.Length < 3 Then
            Return pt
        Else

            If IsBlock = True Then
                Dim DoublePt(pt.Length * 2) As Point3D
                For i As Integer = 0 To pt.Length - 1
                    DoublePt(i) = New Point3D
                    DoublePt(i).x = pt(i).x
                    DoublePt(i).y = pt(i).y
                    DoublePt(i + pt.Length) = New Point3D
                    DoublePt(i + pt.Length).x = pt(i).x
                    DoublePt(i + pt.Length).y = pt(i).y
                Next
                DoublePt(DoublePt.Length - 1) = New Point3D
                DoublePt(DoublePt.Length - 1).x = pt(0).x
                DoublePt(DoublePt.Length - 1).y = pt(0).y

                Dim ResultInsertPt(-1) As Point3D '插值后的结果
                ResultInsertPt = insert(DoublePt)  '插值函数


                'For i As Integer = 0 To u1.Length - 1
                '    ReDim Preserve ResultInsertPt(ResultInsertPt.Length)
                '    ResultInsertPt(ResultInsertPt.Length - 1) = New Point3D
                '    ResultInsertPt(i).x = u1(i)
                '    ResultInsertPt(i).y = v1(i)
                'Next


                Dim ResultSmoothPt(-1) As Point3D  '圆滑后的结果
                Dim start, ended As Integer '润滑的起始位置与结束位置

                Dim StartPt As Point3D  '润滑的起始点
                StartPt.x = pt(pt.Length \ 2).x
                StartPt.y = pt(pt.Length \ 2).y

                '润滑封闭曲线的起始与结束位置
                'Dim k As Integer = 0
                For i As Integer = ResultInsertPt.Length - 1 To 0 Step -1
                    If StartPt.x = ResultInsertPt(i).x And StartPt.y = ResultInsertPt(i).y Then
                        ended = i
                        Exit For
                    End If
                Next

                For i As Integer = 0 To ResultInsertPt.Length - 1
                    If StartPt.x = ResultInsertPt(i).x And StartPt.y = ResultInsertPt(i).y Then
                        start = i
                        Exit For
                    End If
                Next

                '取得构成封闭曲线的点
                For i As Integer = start To ended
                    ReDim Preserve ResultSmoothPt(ResultSmoothPt.Length)
                    ResultSmoothPt(ResultSmoothPt.Length - 1) = New Point3D
                    ResultSmoothPt(ResultSmoothPt.Length - 1).x = ResultInsertPt(i).x
                    ResultSmoothPt(ResultSmoothPt.Length - 1).y = ResultInsertPt(i).y
                Next

                Return ResultSmoothPt
            Else
                Dim DoublePt(pt.Length - 1) As Point3D
                For i As Integer = 0 To pt.Length - 1
                    DoublePt(i) = New Point3D
                    DoublePt(i).x = pt(i).x
                    DoublePt(i).y = pt(i).y
                Next
                Dim ResultInsertPt(-1) As Point3D '插值后的结果
                ResultInsertPt = insert(DoublePt)  '插值函数
                'For i As Integer = 0 To u1.Length - 1
                '    ReDim Preserve ResultInsertPt(ResultInsertPt.Length)
                '    ResultInsertPt(ResultInsertPt.Length - 1) = New Point3D
                '    ResultInsertPt(i).x = u1(i)
                '    ResultInsertPt(i).y = v1(i)
                'Next
                Return ResultInsertPt
            End If
        End If

    End Function

End Class
